#include "csg_type.h"

Vertex3::Vertex3(Vector3 _pos, Vector3 _normal)
{
    pos = Vector3(_pos.x, _pos.y, _pos.z);
    normal = Vector3(_normal.x, _normal.y, _normal.z);
}

void Vertex3::invert()
{
    normal.x = -normal.x;
    normal.y = -normal.y;
    normal.z = -normal.z;
}

Vertex3 Vertex3::interpolate(Vertex3 a, double t)
{
    pos.x += (a.pos.x-pos.x)*t;
    pos.y += (a.pos.y-pos.y)*t;
    pos.z += (a.pos.z-pos.z)*t;
    normal.x += (a.normal.x-normal.x)*t;
    normal.y += (a.normal.y-normal.y)*t;
    normal.z += (a.normal.z-normal.z)*t;
    return Vertex3(pos, normal);
}

AABBBox::AABBBox()
{
    aabb_min = Vector3(CELL_MAX, CELL_MAX, CELL_MAX);
    aabb_max = Vector3(-CELL_MAX, -CELL_MAX, -CELL_MAX);
}

AABBBox::AABBBox(Vector3 _aabb_min, Vector3 _aabb_max)
{
    aabb_min = Vector3(_aabb_min.x, _aabb_min.y, _aabb_min.z);
    aabb_max = Vector3(_aabb_max.x, _aabb_max.y, _aabb_max.z);
}

void AABBBox::addAABB(AABBBox box)
{
    aabb_min.x = min(aabb_min.x, box.aabb_min.x);
    aabb_max.x = max(aabb_max.x, box.aabb_max.x);
    aabb_min.y = min(aabb_min.y, box.aabb_min.y);
    aabb_max.y = max(aabb_max.y, box.aabb_max.y);
    aabb_min.z = min(aabb_min.z, box.aabb_min.z);
    aabb_max.z = max(aabb_max.z, box.aabb_max.z);
}

bool AABBBox::AABBIntersectAABB(AABBBox box)
{
    if (aabb_min.x>box.aabb_max.x || box.aabb_min.x>aabb_max.x)
        return false;
    if (aabb_min.y>box.aabb_max.y || box.aabb_min.y>aabb_max.y)
        return false;
    if (aabb_min.z>box.aabb_max.z || box.aabb_min.z>aabb_max.z)
        return false;
    return true;
}

MyPolygon::MyPolygon(vector<Vertex3> _vertices,double _share)
{
    vertices = _vertices;
    share  =_share;
    normal = Cross(vertices[1].pos-vertices[0].pos, vertices[2].pos-vertices[0].pos);
    normal.Normalize();
//    if (Dot(normal, vertices[0].normal)<0.0) {
//        normal.x = -normal.x;
//        normal.y = -normal.y;
//        normal.z = -normal.z;
//    }
    w= Dot(normal, vertices[0].pos);
    box = AABBBox();
    for (size_t j=0;j<vertices.size();j++) {
        if (box.aabb_max.x<vertices[j].pos.x)
            box.aabb_max.x = vertices[j].pos.x;
        if (box.aabb_max.y<vertices[j].pos.y)
            box.aabb_max.y = vertices[j].pos.y;
        if (box.aabb_max.z<vertices[j].pos.z)
            box.aabb_max.z = vertices[j].pos.z;
        if (box.aabb_min.x>vertices[j].pos.x)
            box.aabb_min.x = vertices[j].pos.x;
        if (box.aabb_min.y>vertices[j].pos.y)
            box.aabb_min.y = vertices[j].pos.y;
        if (box.aabb_min.z>vertices[j].pos.z)
            box.aabb_min.z = vertices[j].pos.z;
    }
}

void MyPolygon::update()
{
    normal = Cross(vertices[1].pos-vertices[0].pos, vertices[2].pos-vertices[0].pos);
    normal.Normalize();
    w= Dot(normal, vertices[0].pos);
    box = AABBBox();
    for (size_t j=0;j<vertices.size();j++) {
        if (box.aabb_max.x<vertices[j].pos.x)
            box.aabb_max.x = vertices[j].pos.x;
        if (box.aabb_max.y<vertices[j].pos.y)
            box.aabb_max.y = vertices[j].pos.y;
        if (box.aabb_max.z<vertices[j].pos.z)
            box.aabb_max.z = vertices[j].pos.z;
        if (box.aabb_min.x>vertices[j].pos.x)
            box.aabb_min.x = vertices[j].pos.x;
        if (box.aabb_min.y>vertices[j].pos.y)
            box.aabb_min.y = vertices[j].pos.y;
        if (box.aabb_min.z>vertices[j].pos.z)
            box.aabb_min.z = vertices[j].pos.z;
    }
}

MyPolygon* MyPolygon::clone()
{
    vector<Vertex3> vertices2= vector<Vertex3>(this->vertices);
    return new MyPolygon(vertices2, this->share);
}

void MyPolygon::invert()
{
    std::reverse(this->vertices.begin(),this->vertices.end());
    for(vector<Vertex3>::iterator it=this->vertices.begin(); it!=this->vertices.end(); ++it)
        it->invert();
    normal.x = -normal.x;
    normal.y = -normal.y;
    normal.z = -normal.z ;
    w = -w;
}

Plane::Plane(Vector3 _normal,double _w)
{
    normal = Vector3(_normal.x, _normal.y, _normal.z);
    w = _w;
}

Plane* Plane::clone()
{
    Vector3 normal2 = Vector3(normal.x, normal.y, normal.z);
    return new Plane(normal2, this->w);
}

void Plane::invert()
{
    normal.x = -normal.x;
    normal.y = -normal.y;
    normal.z = -normal.z ;
    w = -w;
}

void Plane::splitPolygon(MyPolygon *polygon, vector<MyPolygon> &coplanarFront, vector<MyPolygon> &coplanarBack, vector<MyPolygon> &front, vector<MyPolygon> &back)
{
    const int COPLANAR = 0;
    const int FRONT = 1;
    const int BACK = 2;
    const int SPANNING = 3;

    // Classify each point as well as the entire polygon into one of the above
    // four classes.
    int polygonType = 0;

    vector<int> types=vector<int>();
    for (size_t i = 0; i < polygon->vertices.size(); i++) {
        double t = Dot(this->normal, polygon->vertices[i].pos) - this->w;
        int type = (t < -CELL_EPS) ? BACK : (t > CELL_EPS) ? FRONT : COPLANAR;
        polygonType |= type;
        types.push_back(type);
    }

    // Put the polygon in the correct list, splitting it when necessary.
    switch (polygonType) {
        case COPLANAR:
          (Dot(this->normal, polygon->normal) > 0 ? coplanarFront : coplanarBack).push_back(*polygon);
          break;
        case FRONT:
          front.push_back(*polygon);
          break;
        case BACK:
          back.push_back(*polygon);
          break;
        case SPANNING:
          vector<Vertex3> f = vector<Vertex3>();
          vector<Vertex3> b = vector<Vertex3>();
          for (size_t i = 0; i < polygon->vertices.size(); i++) {
              int j = (i + 1) % polygon->vertices.size();
              int ti = types[i];
              int tj = types[j];
              Vertex3 vi = polygon->vertices[i];
              Vertex3 vj = polygon->vertices[j];
              if (ti != BACK) f.push_back(vi);
              if (ti != FRONT) b.push_back(ti != BACK ? Vertex3(vi.pos, vi.normal) : vi);
              if ((ti | tj) == SPANNING) {
                double t = (this->w - Dot(this->normal, vi.pos)) / Dot(this->normal, vj.pos-vi.pos);
                Vertex3 v = vi.interpolate(vj, t);
                f.push_back(v);
                b.push_back(Vertex3(v.pos, v.normal));
              }
          }
          if (f.size() >= 3) front.push_back(MyPolygon(f, polygon->share));
          if (b.size() >= 3) back.push_back(MyPolygon(b, polygon->share));
          f.clear();
          b.clear();
          break;
    }
}

Node::Node(vector<MyPolygon> polygons) {
    this->plane = NULL;
    this->front = NULL;
    this->back = NULL;
    this->build(polygons);
}

Node::Node() {
  this->plane = NULL;
  this->front = NULL;
  this->back = NULL;
}

Node::Node(Node* n) {
    if(n->plane)
        this->plane=n->plane->clone();
    if(n->front)
        this->front=n->front;
    if(n->back)
        this->back=n->back;
    this->polygons=vector<MyPolygon>(n->polygons);
}

Node* Node::clone()
{
    Node* node = new Node(this);
    return node;
}

void Node::invert()
{
    for (size_t i = 0; i < this->polygons.size(); i++) {
        this->polygons[i].invert();
    }
    this->plane->invert();
    if (this->front) this->front->invert();
    if (this->back) this->back->invert();
    Node* temp = this->front;
    this->front = this->back;
    this->back = temp;
}

vector<MyPolygon> Node::clipPolygons(vector<MyPolygon> polygons)
{
    if (!this->plane)
        return vector<MyPolygon>(this->polygons);
    vector<MyPolygon> front=vector<MyPolygon>();
    vector<MyPolygon> back=vector<MyPolygon>();
    for (size_t i = 0; i < polygons.size(); i++) {
        this->plane->splitPolygon(&polygons[i], front, back, front, back);
    }
    if (this->front) front = this->front->clipPolygons(front);
    if (this->back) back = this->back->clipPolygons(back);
    else back.clear();
    vector<MyPolygon> total=vector<MyPolygon>();
    total.reserve(front.size() + back.size()); // preallocate memory
    total.insert( total.end(), front.begin(), front.end() );
    total.insert( total.end(), back.begin(), back.end() );
    front.clear();
    back.clear();
    return total;
}

void Node::clipTo(Node* bsp)
{
    this->polygons = bsp->clipPolygons(this->polygons);
    if (this->front) this->front->clipTo(bsp);
    if (this->back) this->back->clipTo(bsp);
}

vector<MyPolygon> Node::allPolygons()
{
    vector<MyPolygon> polygons =vector<MyPolygon>(this->polygons);
    if (this->front)
    {
        vector<MyPolygon> temp=this->front->allPolygons();
        polygons.reserve(polygons.size() + temp.size());
        polygons.insert( polygons.end(), temp.begin(), temp.end());
        temp.clear();
    }
    if (this->back)
    {
        vector<MyPolygon> temp=this->back->allPolygons();
        polygons.reserve(polygons.size() + temp.size());
        polygons.insert( polygons.begin(), temp.begin(), temp.end());
        temp.clear();
    }
    return polygons;
}

void Node::build(vector<MyPolygon> polygons)
{
    if (polygons.size()==0) return;
    if (!this->plane) this->plane = new Plane(polygons[0].normal, polygons[0].w);
//    if (!this->plane) {
//        if (polygons.size()<100) {
//            this->box = AABBBox();
//            for (size_t i=0;i<polygons.size();i++) {
//                this->box.addAABB(polygons[i].box);
//            }
//            this->plane = new Plane(polygons[0].normal, polygons[0].w);
//        }
//        else {
//            this->box = AABBBox();
//            for (size_t i=0;i<polygons.size();i++) {
//                this->box.addAABB(polygons[i].box);
//            }
//            Vector3 range = box.aabb_max-box.aabb_min;
//            if (range.x>range.y&&range.x>range.z)
//                this->plane = new Plane(Vector3(1.0, 0.0, 0.0), (box.aabb_max.x+box.aabb_min.x)/2);
//            else if (range.y>range.z)
//                this->plane = new Plane(Vector3(0.0, 1.0, 0.0), (box.aabb_max.y+box.aabb_min.y)/2);
//            else
//                this->plane = new Plane(Vector3(0.0, 0.0, 1.0), (box.aabb_max.z+box.aabb_min.z)/2);
//        }
//    }
    vector<MyPolygon> front=vector<MyPolygon>();
    vector<MyPolygon> back=vector<MyPolygon>();
    for (size_t i = 0; i < polygons.size(); i++) {
      this->plane->splitPolygon(&polygons[i], this->polygons, this->polygons, front, back);
    }
    if (front.size()) {
      if (!this->front) this->front = new Node();
      this->front->build(front);
    }
    if (back.size()) {
      if (!this->back) this->back = new Node();
      this->back->build(back);
    }
    front.clear();
    back.clear();
}
